#include "control.h"
#include "bsp_c610.h"
#include "bsp_can.h"
#include "bsp_imu.h"
#include <math.h>
#include "filter.h"

extern imu_t imu;

control_info_t control_info;

// control variable is speed, unit:rpm
pid_t right_motor_speed_pid = 
{
	.kp = 20.f,
	.ki = 100.f,
	.kd = 0.f,
	
	.reference = 0.f,
	.feedback = 0.f,
	.period = 0.002f, 

	
	.integral_limit = 20000,
	.output_limit = 10000,
	.integral_deathzoom = 4000, 
};
//speed pid controler for right motor
pid_t left_motor_speed_pid = 
{
	.kp = 20.f,
	.ki = 100.f,
	.kd = 0.f,
	
	.reference = 0.f,
	.feedback = 0.f,
	.period = 0.002f, 

	
	.integral_limit = 20000,
	.output_limit = 10000,
	.integral_deathzoom = 4000, 
};
//speed pid controler for left motor

pid_t attitude_outer_pid = 
{
	.kp = 5.f,
	.ki = 0.1f,
	.kd = 0.f,
	
	.reference = 0.f,
	.feedback = 0.f,
	.period = 0.002f, 

	
	.integral_limit = 20000,
	.output_limit = 90,
	.integral_deathzoom = 4000, 	
};
//pid controler for attitude outer loop
pid_t attitude_inter_pid = 
{
	.kp = 100.f,
	.ki = 1.f,
	.kd = 0.8f,
	
	.reference = 0.f,
	.feedback = 0.f,
	.period = 0.002f, 

	
	.integral_limit = 1000,
	.output_limit = 100,
	.integral_deathzoom = 200, 	
};
//pid controler for attitude inter loop


float attitude_angle_ref = MECHANICAL_MEDIAN_ANGLE;
//attitude angle reference

void motor_speed_control( float ref_right,float ref_left )
{
	right_motor_speed_pid.reference = ref_right;
	right_motor_speed_pid.feedback = c610_get_speed(RIGHT_MOTOR_ID);
	pid_calculate( &right_motor_speed_pid );
	
	left_motor_speed_pid.reference = ref_left;
	left_motor_speed_pid.feedback = c610_get_speed(LEFT_MOTOR_ID);
	pid_calculate( &left_motor_speed_pid );	
	
	
	if( right_motor_speed_pid.output > M2006_CURRENT_MAX )
	{
		right_motor_speed_pid.output = M2006_CURRENT_MAX;
	}
	else if( right_motor_speed_pid.output < -M2006_CURRENT_MAX )
	{
		right_motor_speed_pid.output = -M2006_CURRENT_MAX;
	}

	if( left_motor_speed_pid.output > M2006_CURRENT_MAX )
	{
		left_motor_speed_pid.output = M2006_CURRENT_MAX;
	}
	else if( left_motor_speed_pid.output < -M2006_CURRENT_MAX )
	{
		left_motor_speed_pid.output = -M2006_CURRENT_MAX;
	}
	
	can1_send_message(0x200,right_motor_speed_pid.output,left_motor_speed_pid.output,0,0);
}


float attitude_pid(float ref_angle)
{
	attitude_outer_pid.reference = ref_angle;
	attitude_outer_pid.feedback = imu.pit;
	pid_calculate( &attitude_outer_pid );
	
	attitude_inter_pid.reference = -attitude_outer_pid.output / 180.f * PI;
	attitude_inter_pid.feedback = imu.wy; 
	pid_calculate( &attitude_inter_pid);
	//attitude control
	return attitude_inter_pid.output;
}


//facing forward  left:+  right:-
//left motor -- close switch  
//right motor
//reverse gyroy -  pitch + 
//move forward to obtain positive angular velocity
//left motor + -> gyro_y - -> car move +
//right motor - -> gyro_y - -> car move +
//retval:unit is rpm
float move_accel;
float mglsin_a;
float Ja;
float mlcos_a;
float control_resolution(float u)
{
	float cos_a = cos( (imu.pit - MECHANICAL_MEDIAN_ANGLE) / 180.f * PI);
	float sin_a = sin( (imu.pit - MECHANICAL_MEDIAN_ANGLE) / 180.f * PI );
	 Ja = -J*u;
	 mglsin_a = WEIGHT * GRAVITY_MSS * L * sin_a;
	 mlcos_a = WEIGHT * L * cos_a;
	
	move_accel = (Ja - mglsin_a) / mlcos_a ;
	return move_accel;
}


float motor_speed_ref = 0;
float kms = 1;
void attitude_control(float dT)
{
	float attitude_pid_output = 0;
	
	//step1:Calculate expected angle -- mileage control
//	attitude_angle_ref = mileage_control(0) + MECHANICAL_MEDIAN_ANGLE;
	
	//step2:attitude pid calculation
	attitude_pid_output = attitude_pid(MECHANICAL_MEDIAN_ANGLE);
	
	//step3:calculate motor speed reference
	motor_speed_ref += control_resolution(attitude_pid_output) * dT * 60 * 36 / PI / WHEEL_DIAMETER ; 
	//control resolution,which output is motor_speed reference
	
	//step4:motor speed control
	//left motor + -> gyro_y - -> car move +
	//right motor - -> gyro_y - -> car move +
	if( motor_speed_ref > M2006_SPEED_ABS_MAX )
	{
		motor_speed_ref = M2006_SPEED_ABS_MAX;
	}
	else if( motor_speed_ref < -M2006_SPEED_ABS_MAX )
	{
		motor_speed_ref = -M2006_SPEED_ABS_MAX;
	}
	control_info.motor_speed_ref = motor_speed_ref;
	motor_speed_control( -1.f * motor_speed_ref , motor_speed_ref );
}


//unit:m
//left motor + -> gyro_y - -> car move +
//right motor - -> gyro_y - -> car move +
//right_mileage & left_mileage:unit is round count
void update_car_mileage(float right_mileage,float left_mileage) 
{
	control_info.mileage = ( left_mileage - right_mileage ) / 2.f / 36.f * PI * WHEEL_DIAMETER;	
}


float mileage_control(float mileage_ref)
{
	float ret;
	float kp = -10.f;
	float error = mileage_ref - control_info.mileage;
	const float max_angle = 20.f;
	
	ret = error * kp;
	if( ret > max_angle )
	{
		ret = max_angle;
	}
	else if( ret < -max_angle )
	{
		ret = -max_angle;
	}
	return ret;
}